#include "eskf.h"
namespace SlamCraft
{
    ESKF::ESKF()
    {
    }

    ESKF::~ESKF()
    {
    }
    void ESKF::predict(const IMU & imu_data,double dt){
        auto imu = imu_data;
        imu.acc -= X.ba;
        imu.gro -= X.bg;
        ///.状态更新
        /**
        ///. like simple lio
        X.rotation = Eigen::Quaterniond(X.rotation.toRotationMatrix()*so3Exp((imu.gro)*dt));
        X.rotation.normalize();
        auto rotation = X.rotation.toRotationMatrix();
        ///. 去除重力影响
        X.velocity += (X.rotation*(imu.acc)+X.gravity)*dt;
        X.position += X.velocity*dt;
        **/
        ///. 全使用当前量更新
        auto rotation = X.rotation.toRotationMatrix();
        X.rotation = Eigen::Quaterniond(X.rotation.toRotationMatrix()*so3Exp((imu.gro)*dt));
        X.rotation.normalize();
        X.position += X.velocity*dt;
        X.velocity += (rotation*(imu.acc)+X.gravity)*dt;


        ///. P递推
        Eigen::Matrix<double,18,18> Fx;

        Eigen::Matrix<double,18 ,12>Fw;
        Fw.setZero();
        Fx.setIdentity();
        Fx.block<3,3>(0,0) = so3Exp(-1*imu.gro*dt);
        ///.paper
        //Fx.block<3,3>(0,9) = -1*A_T(imu.gro*dt)*dt;
        ///.fastlio
        Fx.block<3,3>(0,9) = -1*A_T(-imu.gro*dt)*dt;

        Fx.block<3,3>(3,6) =  Eigen::Matrix3d::Identity()*dt;
        Fx.block<3,3>(6,0) = rotation* skew_symmetric(imu.acc)*dt*(-1);
        //Fx.block<3,3>(6,0) =  skew_symmetric(rotation*imu.acc)*dt*(-1);
        Fx.block<3,3>(6,12) = rotation*dt*(-1);
        Fx.block<3,3>(6,15) = Eigen::Matrix3d::Identity()*dt;
        ///. 近似做法
        //Fw.block<3,3>(0,0) = -Eigen::Matrix3d::Identity()*dt;
        ///. fast_lio做法
        Fw.block<3,3>(0,0) = -1*A_T(-imu.gro*dt)*dt;
        ///. paper 
        //Fw.block<3,3>(0,0) = -1*A_T(imu.gro*dt)*dt;
        Fw.block<3,3>(6,3) = -1*rotation*dt;
        Fw.block<3,3>(9,6) = Fw.block<3,3>(12,9) = Eigen::Matrix3d::Identity()*dt;

        P = Fx*P*Fx.transpose()+Fw*Q*Fw.transpose(); 

    }

    void ESKF::update(){
        auto x_k_k = X;
        auto x_k_last = X;
        ///. 开迭
        Eigen::MatrixXd K;
        Eigen::MatrixXd H_k;
        Eigen::Matrix<double,18,18> P_in_update;
        P_in_update = P;
        Eigen::MatrixXd z_k;
        Eigen::MatrixXd R_inv;// R 直接写死0.001; 
        ///. hk6
        zhr_model(x_k_k,z_k,H_k,R_inv);
        ///.  计算K
        Eigen::MatrixXd H_kt = H_k.transpose();
        //K = (H_kt*R_inv*H_k+P_in_update.inverse()).inverse()*H_kt*R_inv;
        K = (H_kt*H_k+(P_in_update/0.001).inverse()).inverse()*H_kt;
        //CTL_AUTOTP("K SIZE:")<<" rows: "<<K.rows()<<" cols: "<<K.cols()<<ctl::endl;
        //ctl::hg_out()<<K.row(0)<<ctl::endl;
        Eigen::MatrixXd update_x = K*z_k;
        //CTL_AUTOTP("update_x")<<update_x.transpose()<<std::endl;
        x_k_k.rotation = x_k_k.rotation.toRotationMatrix()* so3Exp(update_x.block<3,1>(0,0));
        x_k_k.rotation.normalize();
        x_k_k.position = x_k_k.position+update_x.block<3,1>(3,0);
        x_k_k.velocity = x_k_k.velocity+update_x.block<3,1>(6,0);
        x_k_k.bg = x_k_k.bg+update_x.block<3,1>(9,0);
        x_k_k.ba = x_k_k.ba+update_x.block<3,1>(12,0);
        x_k_k.gravity = x_k_k.gravity+update_x.block<3,1>(15,0);
        X = x_k_k;
        P = (Eigen::Matrix<double,18,18>::Identity()-K*H_k)*P_in_update;
    }
    Eigen::Matrix3d  ESKF::A_T(const Eigen::Vector3d& v){
        Eigen::Matrix3d res;
        double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
        double norm = std::sqrt(squaredNorm);
        if(norm <1e-11){
            res = Eigen::Matrix3d::Identity();
        }
        else{
            res = Eigen::Matrix3d::Identity() + (1 - std::cos(norm)) / squaredNorm * skew_symmetric(v) + (1 - std::sin(norm) / norm) / squaredNorm *  skew_symmetric(v) *  skew_symmetric(v);
        }
        return res;
    }
    Eigen::Matrix<double,18,1> ESKF::getErrorState18(const ESKF::State_18 &s1, const  ESKF::State_18 &s2){
        Eigen::Matrix<double,18,1> es;
        es.setZero();
        es.block<3,1>(0,0) = SO3Log(s2.rotation.toRotationMatrix().transpose() * s1.rotation.toRotationMatrix());
        es.block<3,1>(3,0) = s1.position - s2.position;
        es.block<3,1>(6,0) = s1.velocity - s2.velocity;
        es.block<3,1>(9,0) = s1.bg - s2.bg;
        es.block<3,1>(12,0) = s1.ba - s2.ba;
        es.block<3,1>(15,0) = s1.gravity - s2.gravity;
        return es;
    }

} // namespace SlamCraft
